#include "sys.h"
#include "taskC.h"
#include "app.h"


int RAPEC = 0;
float distanceC = 0;
extern u16 step;
//串口1为:树莓派/jetson nano
//串口2为:机械臂stm32f103
//串口3为:左openmv	
//串口4为:右openmv
//串口5为:前stm32f407(红外对管，超声波),f4蓝牙
//串口6为:小车stm32f103


//机械臂前看
//前进上坡
//白框是否有水果 有-{是否成熟 否-后退，移到第二列 是-抓取}
//无-前进，停在第二个白框/水果前
//机械臂向右看，是否有水果 有-{是否成熟 否-记录，机械臂再前看 是-抓取

int abs(int num)
{
	if(num>=0)
		return num;
	else
		return -1*num;
}

void C_CloseToFruit(int x_target,int y_target,int dir)
{
	int r=0, i=0;
	float speed;
	char x_dir = 'W',y_dir = 'E';
	if(dir<0)
	{
		x_dir = 'E',y_dir = 'w';
		dir = 2;
	}
	if(Fruit_num!=0)
	{	
		for(i=1;i>-1;i--)
		{	
			speed = (-1+dir)*speed_slow*(1+i*1);
			if(target_y>y_target)
			{
				order_num(3, y_dir,-1*speed);	
				while(target_y>y_target)u5_printf("target_y:%.2f\r\n",target_y);
			}
			else
			{
				order_num(3, y_dir,speed);
				while(target_y<y_target)u5_printf("target_y:%.2f\r\n",target_y);
			}
			order(3, 'S'); //让车停下
			delay_ms(500);
		}	
		for(i=1;i>-1;i--)
		{
			speed = (-1+dir)*speed_slow*(1+i*1);
			if(x_pep > x_target)
			{
				order_num(3, x_dir,-1*speed);	
				while(x_pep>x_target)u5_printf("x_pep:%.2f\r\n",x_pep);
			}
			else
			{
				order_num(3, x_dir,speed);		
				while(x_pep<x_target)u5_printf("x_pep:%.2f\r\n",x_pep);
			}
			order(3, 'S'); //让车停下
			delay_ms(500);
		}			
		//判断成熟度
		i=r=0;
		for(int j=0;j<10;j++)
		{
			if(ripe == 0)
			{
				i++;
				u5_printf("       i++     \r\n");
			}
			else if(ripe == 1)
			{
				r++;
				u5_printf("       r++     \r\n");
			}
			delay_ms(100);
		}
		if(i>r)RAPEC = 0;
		else RAPEC = 1;
		u5_printf("i=%d, r=%d\r\n", i, r);
	}
	if(RAPEC)//水果成熟
	{
		u5_printf("Ripe\r\n");
		u5_printf("IT'S RAPE\r\n");
		if(carstate==2)
		{
			u5_printf("上坡抓取\r\n");
			order_num(2, 'F', 1);	
		}
		else
			order_num(2, 'F', 0);
		order_num(2, 'K', 0);
	}
	else if(!RAPEC)   //不成熟
	{
		u5_printf("IT'S NOT RAPE\r\n");
		u5_printf("BACK\r\n");
	}
}

void ActionC_ChangeWay(int column)//column：列数
{
	switch(column){
		case 1:
			order_num(2, 'D', 3);
			u5_printf("Column1\r\n");
			order_num(3, 'E',speed_slow);		//第一、二列向右移动
			while(Cross_num == 0);
			order(3, 'S');
			u5_printf("find white cross!\r\n");
			CloseToCross(42.6,46.9,0);
			u5_printf("Column1\r\n");
			order_num(2, 'D', 3);
			u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.2,'W',0.15);	
			wait_s();
			break;
		case 2:
			u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.4,'W',0.15);	
			wait_s();			
			order_num(2, 'D', 2);
			u5_printf("Column2\r\n");
			u3_printf("~%c%.2f~%c%.2f\r\n",'Y',1.0,'Q',0.15);	
			wait_s();			
		  Turn_180();
			order_num(2, 'D', 3);
			CloseToWall(-1,-1,0.6);					
			CloseToWall(3.70,-1,-1);	
			CloseToCross(53.3,43.9,0);
			u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.2,'W',0.15);	
			wait_s();	
			break;		
	}
}

void action_backC(int column)
{
	u5_printf("ACTION_BACK\r\n");
	order(3,'S');
	order_num(2, 'K', 0);
	if(carstate==2) order_num(3, 'w', speed_slow);
	else order_num(3, 'w', speed_fast);
	while(carstate!=2)
	{
		u5_printf("carstate:%d\r\n", carstate);
	}
	order(3, 'S');	
	if(Rec_num&&Fruit_num==0) CloseToRec(60,60,-1);
	u3_printf("~%c%.2f~%c%.2f\r\n",'Y',0.5,'Q',0.1);
	while(carstate!=3);
	while(carstate==3);
	order_num(3, 'W', speed_slow);
	while(Rec_num==0);
	order(3, 'S');	
	CloseToRec(60,60,-1);
}


//C区前进
int action_frontC(int column)
{
	u8 i;
	char num[3]={"000"};
	for(i=0;i<=4;i++)
	{
		if(i==1&&column==1) CloseToWall(-1,-1,2.18);
		else if(i==1&&column==2) CloseToWall(-1,-1,2.1);
		order_num(2, 'A', 4);
		if(Fruit_num&&ripe){
			SWLED_Flag = 0;	
			if(i!=0) order_num(2, 'L', 2); 
			else order_num(2, 'L', 3);
      SYN_FrameInfo(0,intToString(GrapFruitNum,num),0),SWLED_Flag = 1;
		}
		order_num(2, 'D', 4);
		if(Fruit_num&&ripe){	
			SWLED_Flag = 0;	
			if(i!=0) order_num(2, 'R', 2); 
			else order_num(2, 'R', 3);
      SYN_FrameInfo(0,intToString(GrapFruitNum,num),0),SWLED_Flag = 1;
		}
		if(i!=4) u3_printf("~%c%.2f~%c%.2f\r\n",'X',0.4,'W',0.1),wait_s();
	}			
	return 0;
}


void TaskC(void)
{
	static u16 step_C = 1;
	ActionC_ChangeWay(step_C);//车换至step_C道
	action_frontC(step_C);
	step_C++;
	if(step_C>2)
		step++;
}

